#include "Arduino.h"
#include "ADXL345.h"
#include "Wire.h"

ADXL345::ADXL345(void) {
   _accADDR = 0x53;    // I2C Bus Address of ADXL345 Accelerometer
   _aRANG = 0x31;    // ADXL345 Data Format (Range) Word Address
   _aACCL = 0x32;    // ADXL345 Acceleration Register Base
   _aBAND = 0x2C;    // ADXL345 BW Register Address
   _aPWRC = 0x2D;    // ADXL345 POWER_CTL Register
}

void ADXL345::begin(void) {

  set_power_ctl(0x08);  // Sets Measure Mode ON
  set_format(0x0B);      // 00001011 = full_res, 16g range
 
}

void ADXL345::begin(int i2cAddr) {
  
  _accADDR = i2cAddr;
  set_power_ctl(0x08);  // Sets Measure Mode ON
  set_format(0x0B);      // 00001011 = full_res, 16g range
 
}

void ADXL345::get_accel(void) {
 
  Wire.beginTransmission(_accADDR); // transmit to Accelerometer
  Wire.write(_aACCL);        // Send A/D accel data register address
  Wire.endTransmission();    // stop transmitting
  
  Wire.requestFrom(_accADDR, 6);
  
  byte LSB = Wire.read();  // receive LSB
  byte MSB = Wire.read();
  x_accel = ((int(MSB) << 8) + LSB);
  
  LSB = Wire.read();  // receive LSB
  MSB = Wire.read();
  y_accel = ((int(MSB) << 8) + LSB);
  
  LSB = Wire.read();  // receive LSB
  MSB = Wire.read();
  z_accel = ((int(MSB) << 8) + LSB);

}

void ADXL345::set_format(byte theFormat) {

  // D7        | D6  | D5      | D4 | D3       | D2      | D1 | D0 |
  // self test | SPI | int_inv | 0  | full_res | Justify | Range   |
  // full_res = 1 => 13 bit res, LSB = 4 mG; 0 => 10 bit
  // Range => 0 = 2G, 1 = 4G, 2 = 8G, 3 = 16G
  Wire.beginTransmission(_accADDR); // transmit to Altimeter
  Wire.write(_aRANG);        // Send range register
  Wire.write(theFormat);
  Wire.endTransmission();    // stop transmitting

}

int ADXL345::get_format(void) {

  // Read current value of range register
  // D7        | D6  | D5      | D4 | D3       | D2      | D1 | D0 |
  // self test | SPI | int_inv | 0  | full_res | Justify | Range   |
  Wire.beginTransmission(_accADDR); // transmit to Altimeter
  Wire.write(_aRANG);        // Send range register address
  Wire.endTransmission();    // stop transmitting

  Wire.requestFrom(_accADDR, 1);
  int data_format = int(Wire.read());

  return data_format;
}

void ADXL345::set_power_ctl(byte power_ctl) {

  Wire.beginTransmission(_accADDR); // transmit to Altimeter
  Wire.write(_aPWRC);        // Send power_ctl register
  Wire.write(power_ctl);
  Wire.endTransmission();    // stop transmitting

}

int ADXL345::get_power_ctl(void) {

  // Read current value of range register
  // D7 | D6 | D5   | D4          | D3      | D2    | D1 | D0 |
  // 0  | 0  | Link | Auto_Sleep  | Measure | Sleep | Wakeup  |
  Wire.beginTransmission(_accADDR); // transmit to Altimeter
  Wire.write(_aPWRC);        // Send range register address
  Wire.endTransmission();    // stop transmitting

  Wire.requestFrom(_accADDR, 1);
  int data_format = int(Wire.read());

  return data_format;
}

void ADXL345::set_bw_rate(byte bw) {

  Wire.beginTransmission(_accADDR); // transmit to Altimeter
  Wire.write(_aBAND);        // Send power_ctl register
  Wire.write(bw);
  Wire.endTransmission();    // stop transmitting

}

int ADXL345::get_bw_rate(void) {

  // Read current value of BW_RATE register
  // D7 - D5 | D4        | D3 - D0  |
  //  0 0 0  | Low_power | BW/Rate  |
  Wire.beginTransmission(_accADDR); // transmit to Altimeter
  Wire.write(_aBAND);        // Send range register address
  Wire.endTransmission();    // stop transmitting

  Wire.requestFrom(_accADDR, 1);
  int data_format = int(Wire.read());

  return data_format;
}
